#ifndef _SERVO_H
#define _SERVO_H

#include "main.h"
#include "tim.h"
#include "math.h"
#include "LED.h"
#include "kinematics.h"
#include "configuration.h"


#define Axis_Angle_Max 180      //关机角度最大极限值
#define Axis_Angle_Min 0        //关机角度最小极限值
#define GRIPPER_Angle_Max 100   //夹爪左转打开极限值
#define GRIPPER_Angle_Min 50    //夹爪右转闭合极限值
#define Servo_num 6             //舵机数量
#define Axis_num 5              //关节数量

typedef enum {
    AXIS_ALL_ARMS=0,          // 所有关节
    AXIS_BASE_ROTATE,     // 基座旋转（轴1） 正
    AXIS_SHOULDER_PITCH,    // 肩部俯仰（轴2）正
    AXIS_ELBOW_PITCH,       // 肘部俯仰（轴3）反
    AXIS_WRIST_PITCH,       // 腕部俯仰（轴4）正
    AXIS_WRIST_ROTATE,      // 腕部旋转（轴5）0°为左旋，180°为右旋
    AXIS_GRIPPER_ROTATE,    // 机械爪旋转（轴6）100°为开，45°为关
} RobotAxis;

// 定义消息结构体
typedef struct {
    uint64_t timeout;
    KinematicsState state;
} GripperCommand;

extern uint8_t Position_state[5];

extern double home_angles[6];
extern double last_angles[6];
extern double now_temp_angles[6];

void Servo_Init(void);
void Servo_SetAngle(RobotAxis axis,double angle);
void RobotAxis_Init(double now_angles[]);
void Servo_Test(void);
void Axis_remaining_bais(JointAngles *jointangles,double last_angles[],double theta_angles[]);
uint8_t Position_Succes(uint8_t Position_state[]);
void Position_Clear(void);
void Robot_jointAxis_SetAngle(double now_angles[]);
void Axis_Speed_Limit(double *steer_out, double *limit_angle, float limit);
void Axis_now_angle_updata(double now_temp_angles[],double last_angles[],double limit_angles[]);
uint8_t Axis_angle_limit(double now_angles[]);
void Gripper_Open(void);
void Gripper_Close(double angle);
void Gripper_Angle_Limit_Control(double angle);
void Timer_gripper_control(uint64_t timeout,KinematicsState Kinematics_state);

#endif

